/*
  Main Program Loop
*/

#include <msp430x16x.h>
#include <signal.h>

#include "main.h"

#include "log/log.h"
#include "timing/timing.h"
#include "recovery/recover.h"
#include "i2c/i2c.h"
#include "sd/sd.h"
#include "payload/pay.h"
#include "coms/coms.h"
#include "adcs/adcs.h"
#include "adc/adc.h"

/* "Scrubs" values; returns number of bits by which two integers differ */
int valscrub(int a, int b){
  int c,i,diff;
  
  c=a^b; //XOR a and b
  diff=0;

  for(i=0;i<16;i++) {
    diff+=c & 0x0001;
    c=c>>1;
  }

  return diff;
}

/* Clears the command variables */
void reset_command() {
  command=0;
  significance=0;
  commandToken=0;
}

/* Mode functions */
int doRecover() {
  //waitms(100);
  statusVar[SV_TIMEOUT]++;
  if (statusVar[SV_TIMEOUT]==0xFFFF) return MODE_AUTONOMOUS;
  return MODE_RECOVER;
}

int doIdle() {
  return MODE_IDLE;
}

int doOperation() {
  unsigned long int address;
  long int i;

  //Added for payload event simulation July 2010
  //P2OUT=255-P2OUT;

  address=((unsigned long int)statusVar[SV_ADDR1])<<16+(unsigned long int)statusVar[SV_ADDR2];
  statusVar[SV_ACT_SDCARD]=1; //Always use logging, for 3 system test

  /* Logging by whichever means are available */
  chooseLog();

  statusVar[SV_ADDR1]=((unsigned short int)((address&0xFFFF0000)>>16));
  statusVar[SV_ADDR2]=((unsigned short int)(address&0x0000FFFF));

  for(i=0;i<100000;i++)
    nop();

  return MODE_OPERATION;
}

int doAutonomous() {
  /* Logging in RAM only */
  //waitms(LONG_PERIOD);
  statusVar[SV_MINILOG]+=minilog_entry(&minilog[statusVar[SV_MINILOG]]);
  if (statusVar[SV_MINILOG]>=MAXMINILOG) statusVar[SV_MINILOG]=0;

  return MODE_AUTONOMOUS;
}

/* Entry point */
int main() {
  int mode, modem;

  mode=MODE_RECOVER;

  P2DIR=255;

  reset_command();

  eint(); //Enable interrupts

  P1DIR=0;
  P1IE=0xFF;

  /* Initialisations (may need to be moved somewhere else) */
  sdInit();
  //comsInit();
  adcInit();

  /*
  modem = modInit();
  if(modem!=0) return 1; // Terminate if modem has not been setup correctly
  
  modem = modENQ0();
  if(modem!=0) return 1; // Terminate if modem has not been setup correctly
  */

  mode=MODE_OPERATION; //Added for payload event simulation July 2010

  //This is the main loop. It is the core of all OBDH functionality
  while (1) {
    P2OUT=0;
    /* Call the appropriate mode function */
    switch (mode) {
    case MODE_RECOVER: 
      mode=doRecover(); 
      P2OUT=0x03;
      break;

    case MODE_IDLE: 
      mode=doIdle();
      P2OUT=0x0C; 
      break;

    case MODE_OPERATION: 
      mode=doOperation(); 
      P2OUT=0x30;
      break;

    case MODE_AUTONOMOUS: 
      mode=doAutonomous(); 
      P2OUT=0xC0;
      break;

    default:
      if (valscrub(mode, MODE_IDLE)==1) mode=MODE_IDLE;
      if (valscrub(mode, MODE_OPERATION)==1) mode=MODE_OPERATION;
      if (valscrub(mode, MODE_AUTONOMOUS)==1) mode=MODE_AUTONOMOUS;
      mode=MODE_RECOVER;
      break;
    }
    /*
    // Buffer incoming commands 
    if (mod_rec==2) {
      commandBuffer[significance++] = mod_out;
    }

    // Act on commands
    if (significance==4) {
      command=(int)commandBuffer[0]+((int)(commandBuffer[1]))<<8;
      argument=(int)commandBuffer[1]+((int)(commandBuffer[2]))<<8;
      if (command & mode) {
        statusVar[0]=command;
        statusVar[1]=argument;
      }
      if ((command & mode) == CMD_IDLE) mode=MODE_IDLE;
      if ((command & mode) == CMD_RESET) mode=MODE_RECOVER;
      if ((command & mode) == CMD_WAKE) mode=MODE_OPERATION;
      reset_command();
      } 
*/
  }
}
